package tests;

import java.awt.image.BufferedImage;

import processing.core.*;
import processing.opengl.*;

import com.googlecode.javacpp.Loader;
import com.googlecode.javacv.*;
import com.googlecode.javacv.cpp.*;
import com.googlecode.javacv.FrameGrabber.Exception;
import com.googlecode.javacv.cpp.cvkernels;
import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvMemStorage;
import com.googlecode.javacv.cpp.opencv_core.IplImage;

import static com.googlecode.javacv.cpp.opencv_core.*;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_GAUSSIAN;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_RGB2GRAY;
import static com.googlecode.javacv.cpp.opencv_imgproc.CV_THRESH_BINARY;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvCvtColor;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvSmooth;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvThreshold;

@SuppressWarnings({ "serial", "unused" })
public class CameraMotionInteration extends PApplet{
	
	public final int WIDTH = 640;
	public final int HEIGHT = 480;
	
	private int i = 0;
	
	private CameraThread cameraThread;
	
	private Boid[] boids;
	private int numberOfBoids = 500;
	
	int quiverSize = 20;
	
	public void setup()
	{
		size(WIDTH, HEIGHT, OPENGL);

		cameraThread = new CameraThread(20, "Camera1");
		cameraThread.start();
		
		boids = new Boid[numberOfBoids];
		for(int i = 0; i < numberOfBoids; i ++)
		{
			boids[i] = new Boid(this);
		}
	}
	
	public void draw()
	{		
		stroke(0,0,0);
		rect(0, 0, WIDTH, HEIGHT);
		for(int i = 0; i < numberOfBoids; i ++)
		{
			if (cameraThread.isRunning())
			{
				int boidX = (int) boids[i].getPosition().x;
				int boidY = (int) boids[i].getPosition().y;
				PVector acc = cameraThread.getGradientVector(boidX, boidY);
				boids[i].update(acc);			
			}
			else
			{
				boids[i].update(new PVector());				
			}
			boids[i].draw();
		}
		
		if (cameraThread.isRunning())
		{
			for(int x = 0; x < WIDTH; x += quiverSize)
			{
				for(int y = 0; y < HEIGHT; y += quiverSize)
				{
					PVector a = cameraThread.getGradientVector(x, y);
					stroke(0, 255, 0);
					line(x, y, x + a.x * quiverSize, y + a.y * quiverSize);
					stroke(255, 0, 0);
					ellipse(x,y,2,2);
				}
			}			
		}
		
	}
	
	public class Boid
	{		
		PApplet parent;
		
		private PVector position;
		private PVector velocity;
		private PVector acceleration;
		
		private float velocityDamp = 0.999f;
		private float accelerationDamp = 0.2f;
		
		private float size = 5;
		
		public Boid(PApplet p)
		{			
			this(p, new PVector(random(WIDTH),random(HEIGHT)), 
					new PVector(1f - random(2f),1f - random(2f)));
		}
		
		public Boid(PApplet p, PVector pos, PVector vel)
		{
			parent = p;
			
			position = pos;
			velocity = vel;
			acceleration = new PVector();
		}
		
		public void update(PVector acc)
		{
			acceleration.add(acc);
			
			velocity.add(acceleration);
			acceleration.mult(accelerationDamp);
			
			position.add(velocity);
			velocity.mult(velocityDamp);
			
			if(position.x < 0 || position.x > WIDTH)
			{
				position.x = position.x % WIDTH;
			}
			if(position.y < 0 || position.y > HEIGHT)
			{
				position.y = position.y % HEIGHT;
			}
		}
		
		public PVector getPosition()
		{
			return new PVector(position.x,position.y);
		}
		
		public void draw()
		{
			parent.ellipse(position.x, position.y, size, size);
		}
	}
	
	public class CameraThread extends Thread
	{
		private boolean running;
		private int wait;
		private String id;
		  
		OpenCVFrameGrabber grabber = new OpenCVFrameGrabber(0);
		IplImage frame;
		IplImage image = null;
		IplImage previousImage = null;
		IplImage diff = null;
		IplImage threshold = null;
		BufferedImage bufferedDiff = null;
		
		CanvasFrame canvasFrame = new CanvasFrame("Some Title");
		CvMemStorage storage = CvMemStorage.create();
		 
		CameraThread (int w, String s) 
		{
			wait = w;
			id = s;
			running = false;
		}
		 
		public void start () {
			super.start();
		}		 
		 
		public void run () 
		{
	        try {
				grabber.start();
		        frame = grabber.grab();
		        canvasFrame.setCanvasSize(frame.width(), frame.height());

				while (canvasFrame.isVisible() && (frame = grabber.grab()) != null)
				{
					threshold = IplImage.create(frame.width(), frame.height(), IPL_DEPTH_8U, 1);
	                cvCvtColor(frame, threshold, CV_RGB2GRAY);
	    			// do some threshold for wipe away useless details
	                
	                cvThreshold(threshold, threshold, 150, 255, CV_THRESH_BINARY);
	                
	                /*
	                cvSmooth(threshold, threshold, CV_GAUSSIAN, 13, 13, 7, 7);
					cvSmooth(threshold, threshold, CV_GAUSSIAN, 13, 13, 7, 7);
					*/
		            cvSmooth(threshold, threshold, CV_GAUSSIAN, 23, 23, 15, 15);
		            cvSmooth(threshold, threshold, CV_GAUSSIAN, 23, 23, 15, 15);
	                canvasFrame.showImage(threshold);
	                
	    			running = true;
		            
					try 
					{
						sleep((long)(wait));
					} catch (InterruptedException e) 
					{
						e.printStackTrace();
					}
				} 
		        grabber.stop();
		        canvasFrame.dispose();
			} 
	        catch (Exception e) 
			{
				e.printStackTrace();
			}
		}
		
		public boolean isRunning()
		{
			return running;
		}
		
		public PVector getGradientVector(int gradientX, int gradientY)
		{
			
			double xDiff = 0;
			double[][] xSobelKernel ={
					{-1d,-2d,-1d},
					{ 0d, 0d, 0d},
					{ 1d, 2d, 1d}};	
			
			double yDiff = 0;
			double[][] ySobelKernel = {
					{-1d, 0d, 1d},
					{-2d, 0d, 2d},
					{-1d, 0d, 1d}};
			
			double pixelValue;
			
			for(int x = 0; x < 3; x ++)
			{
				for(int y = 0; y < 3; y ++)
				{
					int gx = gradientX / 2 + x - 1;
					int gy = gradientY / 2 + y - 1;
					
					if(gx < 0 || gx >= threshold.width() || gy < 0 || gy >= threshold.height() || threshold == null)
					{
						pixelValue = 0d;
					}
					else
					{
						pixelValue = cvGet2D(threshold, gy, gx).getVal(0);
					}
					xDiff += pixelValue  * xSobelKernel[x][y];
					yDiff += pixelValue  * ySobelKernel[x][y];
				}
			}
			xDiff /= 255 * 4;
			yDiff /= 255 * 4;
			
			return new PVector((float)xDiff, (float)yDiff);
		}
		 
		public void quit() 
		{
			running = false;
			interrupt();
		}
	}

}